#include "Eigen/Dense"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"

// clang-format off
const std::array<double, 9UL> K = {
        640, 0.0, 640.0,
        0.0, 360, 360.0,
        0.0, 0.0, 1.0
    };  // 视场角 90 度
const int width = 1280,
          height = 720;
// clang-format on

class CameraNode : public rclcpp::Node {
   public:
    CameraNode();

   private:
    std::vector<Eigen::Vector2d> armorPoint[2];  // 装甲板在相机上的角点坐标(取最近两块)

    std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
    std::shared_ptr<tf2_ros::TransformListener> tfListener_;

    std::shared_ptr<tf2_ros::TransformBroadcaster> cameraFrameBroadcaster_;

    rclcpp::TimerBase::SharedPtr cameraTimer_;
    rclcpp::TimerBase::SharedPtr cameraFrameBroadcasterTimer_;

    sensor_msgs::msg::CameraInfo cameraInfo_;

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr imagePublisher_;
    rclcpp::TimerBase::SharedPtr imagePublisherTimer_;
};